using System;
using System.Diagnostics;

namespace iStudio.Library.Dsp
{
    // Comments? Questions? Bugs? Tell Ben Houston at ben@exocortex.org
    // Version: May 4, 2002

    /// <summary>
    /// <p>Static functions for doing various Fourier Operations.</p>
    /// </summary>
    public class Fourier
    {
        private const int CMaxLength = 4096;
        private const int CMinLength = 1;

        private const int CMaxBits = 12;
        private const int CMinBits = 0;
        private static readonly int[][] ReversedBits = new int[CMaxBits][];
        private static int[][] _reverseBits;
        private static int _lookupTabletLength = -1;
        private static double[,][] _uRLookup;
        private static double[,][] _uILookup;
        private static float[,][] _uRLookupF;
        private static float[,][] _uILookupF;

        private static void Swap (ref float a, ref float b)
        {
            float temp = a;
            a = b;
            b = temp;
        }

        private static bool IsPowerOf2 (int x)
        {
            return (x & (x - 1)) == 0;
        }

        private static int Pow2 (int exponent)
        {
            if (exponent >= 0 && exponent < 31)
            {
                return 1 << exponent;
            }
            return 0;
        }

        private static int Log2 (int x)
        {
            if (x <= 65536)
            {
                if (x <= 256)
                {
                    if (x <= 16)
                    {
                        if (x <= 4)
                        {
                            if (x <= 2)
                            {
                                return (x <= 1) ? 0 : 1;
                            }

                            return 2;
                        }

                        return (x <= 8) ? 3 : 4;
                    }

                    if (x <= 64)
                    {
                        return (x <= 32) ? 5 : 6;
                    }

                    if (x <= 128)
                    {
                        return 7;
                    }
                    return 8;
                }
                if (x <= 4096)
                {
                    if (x <= 1024)
                    {
                        return (x <= 512) ? 9 : 10;
                    }

                    return (x <= 2048) ? 11 : 12;
                }

                if (x <= 16384)
                {
                    return (x <= 8192) ? 13 : 14;
                }

                return (x <= 32768) ? 15 : 16;
            }
            if (x <= 16777216)
            {
                if (x <= 1048576)
                {
                    if (x <= 262144)
                    {
                        return (x <= 131072) ? 17 : 18;
                    }

                    return (x <= 524288) ? 19 : 20;
                }

                if (x <= 4194304)
                {
                    return (x <= 2097152) ? 21 : 22;
                }

                if (x <= 8388608)
                {
                    return 23;
                }
                return 24;
            }

            if (x <= 268435456)
            {
                if (x <= 67108864)
                {
                    return (x <= 33554432) ? 25 : 26;
                }

                return (x <= 134217728) ? 27 : 28;
            }

            if (x <= 1073741824)
            {
                return (x <= 536870912) ? 29 : 30;
            }
            //	since int is unsigned it can never be higher than 2,147,483,647
            //	if( x <= 2147483648 )
            //		return	31;	
            //	return	32;	
            return 31;
        }

        private static int[] GetReversedBits (int numberOfBits)
        {
            Debug.Assert (numberOfBits >= CMinBits);
            Debug.Assert (numberOfBits <= CMaxBits);
            if (ReversedBits[numberOfBits - 1] == null)
            {
                int maxBits = Pow2 (numberOfBits);
                int[] reversedBits = new int[maxBits];
                for (int i = 0; i < maxBits; i ++)
                {
                    int oldBits = i;
                    int newBits = 0;
                    for (int j = 0; j < numberOfBits; j ++)
                    {
                        newBits = (newBits << 1) | (oldBits & 1);
                        oldBits = (oldBits >> 1);
                    }
                    reversedBits[i] = newBits;
                }
                ReversedBits[numberOfBits - 1] = reversedBits;
            }
            return ReversedBits[numberOfBits - 1];
        }

        private static void ReorderArray (float[] data)
        {
            Debug.Assert (data != null);

            int length = data.Length / 2;

            Debug.Assert (IsPowerOf2 (length));
            Debug.Assert (length >= CMinLength);
            Debug.Assert (length <= CMaxLength);

            int[] reversedBits = GetReversedBits (Log2 (length));
            for (int i = 0; i < length; i ++)
            {
                int swap = reversedBits[i];

                if (swap > i)
                {
                    Swap (ref data[(i << 1)], ref data[(swap << 1)]);
                    Swap (ref data[(i << 1) + 1], ref data[(swap << 1) + 1]);
                }
            }
        }

        private static void ReorderArray (Complex[] data)
        {
            Debug.Assert (data != null);

            int length = data.Length;

            Debug.Assert (IsPowerOf2 (length));
            Debug.Assert (length >= CMinLength);
            Debug.Assert (length <= CMaxLength);

            int[] reversedBits = GetReversedBits (Log2 (length));
            for (int i = 0; i < length; i ++)
            {
                int swap = reversedBits[i];
                if (swap > i)
                {
                    Complex temp = data[i];
                    data[i] = data[swap];
                    data[swap] = temp;
                }
            }
        }

        private static void ReorderArray (ComplexF[] data)
        {
            Debug.Assert (data != null);

            int length = data.Length;

            Debug.Assert (IsPowerOf2 (length));
            Debug.Assert (length >= CMinLength);
            Debug.Assert (length <= CMaxLength);

            int[] reversedBits = GetReversedBits (Log2 (length));
            for (int i = 0; i < length; i ++)
            {
                int swap = reversedBits[i];
                if (swap > i)
                {
                    ComplexF temp = data[i];
                    data[i] = data[swap];
                    data[swap] = temp;
                }
            }
        }

        private static int ReverseBits (int bits, int n)
        {
            int bitsReversed = 0;
            for (int i = 0; i < n; i ++)
            {
                bitsReversed = (bitsReversed << 1) | (bits & 1);
                bits = (bits >> 1);
            }
            return bitsReversed;
        }

        private static void InitializeReverseBits (int levels)
        {
            _reverseBits = new int[levels + 1][];
            for (int j = 0; j < (levels + 1); j ++)
            {
                int count = (int) Math.Pow (2, j);
                _reverseBits[j] = new int[count];
                for (int i = 0; i < count; i ++)
                {
                    _reverseBits[j][i] = ReverseBits (i, j);
                }
            }
        }

        private static void SyncLookupTableLength (int length)
        {
            Debug.Assert (length < 1024 * 10);
            Debug.Assert (length >= 0);
            if (length > _lookupTabletLength)
            {
                int level = (int) Math.Ceiling (Math.Log (length, 2));
                InitializeReverseBits (level);
                InitializeComplexRotations (level);
                //_cFFTData	= new Complex[ Math2.CeilingBase( length, 2 ) ];
                //_cFFTDataF	= new ComplexF[ Math2.CeilingBase( length, 2 ) ];
                _lookupTabletLength = length;
            }
        }

        private static void InitializeComplexRotations (int levels)
        {
            int ln = levels;
            //_wRLookup = new float[ levels + 1, 2 ];
            //_wILookup = new float[ levels + 1, 2 ];

            _uRLookup = new double[levels + 1,2][];
            _uILookup = new double[levels + 1,2][];

            _uRLookupF = new float[levels + 1,2][];
            _uILookupF = new float[levels + 1,2][];

            int n = 1;
            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                //float scale = (float)( 1 / Math.Sqrt( 1 << ln ) );

                // positive sign ( i.e. [M,0] )
                {
                    double uR = 1;
                    double uI = 0;
                    double angle = Math.PI / m * 1;
                    double wR = Math.Cos (angle);
                    double wI = Math.Sin (angle);

                    _uRLookup[level, 0] = new double[m];
                    _uILookup[level, 0] = new double[m];
                    _uRLookupF[level, 0] = new float[m];
                    _uILookupF[level, 0] = new float[m];

                    for (int j = 0; j < m; j ++)
                    {
                        _uRLookupF[level, 0][j] = (float) (_uRLookup[level, 0][j] = uR);
                        _uILookupF[level, 0][j] = (float) (_uILookup[level, 0][j] = uI);
                        double uwI = uR * wI + uI * wR;
                        uR = uR * wR - uI * wI;
                        uI = uwI;
                    }
                }
                {
                    // negative sign ( i.e. [M,1] )
                    double uR = 1;
                    double uI = 0;
                    double angle = Math.PI / m * -1;
                    double wR = Math.Cos (angle);
                    double wI = Math.Sin (angle);

                    _uRLookup[level, 1] = new double[m];
                    _uILookup[level, 1] = new double[m];
                    _uRLookupF[level, 1] = new float[m];
                    _uILookupF[level, 1] = new float[m];

                    for (int j = 0; j < m; j ++)
                    {
                        _uRLookupF[level, 1][j] = (float) (_uRLookup[level, 1][j] = uR);
                        _uILookupF[level, 1][j] = (float) (_uILookup[level, 1][j] = uI);
                        double uwI = uR * wI + uI * wR;
                        uR = uR * wR - uI * wI;
                        uI = uwI;
                    }
                }
            }
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers (as pairs of float's).
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void Fft (float[] data, int length, FourierDirection direction)
        {
            Debug.Assert (data != null);
            Debug.Assert (data.Length >= length * 2);
            Debug.Assert (IsPowerOf2 (length));

            SyncLookupTableLength (length);

            int ln = Log2 (length);

            // reorder array
            ReorderArray (data);

            // successive doubling
            int n = 1;
            int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;
            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                float[] uRLookup = _uRLookupF[level, signIndex];
                float[] uILookup = _uILookupF[level, signIndex];

                for (int j = 0; j < m; j ++)
                {
                    float uR = uRLookup[j];
                    float uI = uILookup[j];

                    for (int evenT = j; evenT < length; evenT += n)
                    {
                        int even = evenT << 1;
                        int odd = (evenT + m) << 1;

                        float r = data[odd];
                        float i = data[odd + 1];

                        float odduR = r * uR - i * uI;
                        float odduI = r * uI + i * uR;

                        r = data[even];
                        i = data[even + 1];

                        data[even] = r + odduR;
                        data[even + 1] = i + odduI;

                        data[odd] = r - odduR;
                        data[odd + 1] = i - odduI;
                    }
                }
            }
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers (as pairs of float's).
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void FftQuick (float[] data, int length, FourierDirection direction)
        {
            /*Debug.Assert( data != null );
			Debug.Assert( data.Length >= length*2 );
			Debug.Assert( Fourier.IsPowerOf2( length ) == true );

			Fourier.SyncLookupTableLength( length );*/

            int ln = Log2 (length);

            // reorder array
            ReorderArray (data);

            // successive doubling
            int n = 1;
            int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;
            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                float[] uRLookup = _uRLookupF[level, signIndex];
                float[] uILookup = _uILookupF[level, signIndex];

                for (int j = 0; j < m; j ++)
                {
                    float uR = uRLookup[j];
                    float uI = uILookup[j];

                    for (int evenT = j; evenT < length; evenT += n)
                    {
                        int even = evenT << 1;
                        int odd = (evenT + m) << 1;

                        float r = data[odd];
                        float i = data[odd + 1];

                        float odduR = r * uR - i * uI;
                        float odduI = r * uI + i * uR;

                        r = data[even];
                        i = data[even + 1];

                        data[even] = r + odduR;
                        data[even + 1] = i + odduI;

                        data[odd] = r - odduR;
                        data[odd + 1] = i - odduI;
                    }
                }
            }
        }

        public static void MyFft (bool forward, int m, ComplexF[] data)
        {
            int i;
            int l;

            // Calculate the number of points
            int n = 1;
            for (i = 0; i < m; i++)
            {
                n *= 2;
            }

            // Do the bit reversal
            int i2 = n >> 1;
            int j = 0;
            for (i = 0; i < n - 1; i++)
            {
                if (i < j)
                {
                    float tx = data[i].Re;
                    float ty = data[i].Im;
                    data[i].Re = data[j].Re;
                    data[i].Im = data[j].Im;
                    data[j].Re = tx;
                    data[j].Im = ty;
                }
                int k = i2;

                while (k <= j)
                {
                    j -= k;
                    k >>= 1;
                }
                j += k;
            }

            // Compute the FFT 
            float c1 = -1.0f;
            float c2 = 0.0f;
            int l2 = 1;
            for (l = 0; l < m; l++)
            {
                int l1 = l2;
                l2 <<= 1;
                float u1 = 1.0f;
                float u2 = 0.0f;
                for (j = 0; j < l1; j++)
                {
                    for (i = j; i < n; i += l2)
                    {
                        int i1 = i + l1;
                        float t1 = u1 * data[i1].Re - u2 * data[i1].Im;
                        float t2 = u1 * data[i1].Im + u2 * data[i1].Re;
                        data[i1].Re = data[i].Re - t1;
                        data[i1].Im = data[i].Im - t2;
                        data[i].Re += t1;
                        data[i].Im += t2;
                    }
                    float z = u1 * c1 - u2 * c2;
                    u2 = u1 * c2 + u2 * c1;
                    u1 = z;
                }

                c2 = (float) Math.Sqrt ((1.0f - c1) / 2.0f);

                if (forward)
                {
                    c2 = -c2;
                }
                c1 = (float) Math.Sqrt ((1.0f + c1) / 2.0f);
            }

            // Scaling for forward transform 
            if (forward)
            {
                for (i = 0; i < n; i++)
                {
                    data[i].Re /= n;
                    data[i].Im /= n;
                }
            }
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void Fft (ComplexF[] data, int length, FourierDirection direction)
        {
            if (data == null)
            {
                throw new ArgumentNullException ("data");
            }
            if (data.Length < length)
            {
                throw new ArgumentOutOfRangeException ("length", length, "must be at least as large as 'data.Length' parameter");
            }
            if (IsPowerOf2 (length) == false)
            {
                throw new ArgumentOutOfRangeException ("length", length, "must be a power of 2");
            }

            SyncLookupTableLength (length);

            int ln = Log2 (length);

            // reorder array
            ReorderArray (data);

            // successive doubling
            int n = 1;
            int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;

            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                float[] uRLookup = _uRLookupF[level, signIndex];
                float[] uILookup = _uILookupF[level, signIndex];

                for (int j = 0; j < m; j ++)
                {
                    float uR = uRLookup[j];
                    float uI = uILookup[j];

                    for (int even = j; even < length; even += n)
                    {
                        int odd = even + m;

                        float r = data[odd].Re;
                        float i = data[odd].Im;

                        float odduR = r * uR - i * uI;
                        float odduI = r * uI + i * uR;

                        r = data[even].Re;
                        i = data[even].Im;

                        data[even].Re = r + odduR;
                        data[even].Im = i + odduI;

                        data[odd].Re = r - odduR;
                        data[odd].Im = i - odduI;
                    }
                }
            }
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void FftQuick (ComplexF[] data, int length, FourierDirection direction)
        {
            /*if( data == null ) {
				throw new ArgumentNullException( "data" );
			}
			if( data.Length < length ) {
				throw new ArgumentOutOfRangeException( "length", length, "must be at least as large as 'data.Length' parameter" );
			}
			if( Fourier.IsPowerOf2( length ) == false ) {
				throw new ArgumentOutOfRangeException( "length", length, "must be a power of 2" );
			}

			Fourier.SyncLookupTableLength( length );*/

            int ln = Log2 (length);

            // reorder array
            ReorderArray (data);

            // successive doubling
            int n = 1;
            int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;

            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                float[] uRLookup = _uRLookupF[level, signIndex];
                float[] uILookup = _uILookupF[level, signIndex];

                for (int j = 0; j < m; j ++)
                {
                    float uR = uRLookup[j];
                    float uI = uILookup[j];

                    for (int even = j; even < length; even += n)
                    {
                        int odd = even + m;

                        float r = data[odd].Re;
                        float i = data[odd].Im;

                        float odduR = r * uR - i * uI;
                        float odduI = r * uI + i * uR;

                        r = data[even].Re;
                        i = data[even].Im;

                        data[even].Re = r + odduR;
                        data[even].Im = i + odduI;

                        data[odd].Re = r - odduR;
                        data[odd].Im = i - odduI;
                    }
                }
            }
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="direction"></param>
        public static void Fft (ComplexF[] data, FourierDirection direction)
        {
            if (data == null)
            {
                throw new ArgumentNullException ("data");
            }
            Fft (data, data.Length, direction);
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void Fft (Complex[] data, int length, FourierDirection direction)
        {
            if (data == null)
            {
                throw new ArgumentNullException ("data");
            }
            if (data.Length < length)
            {
                throw new ArgumentOutOfRangeException ("length", length, "must be at least as large as 'data.Length' parameter");
            }
            if (IsPowerOf2 (length) == false)
            {
                throw new ArgumentOutOfRangeException ("length", length, "must be a power of 2");
            }

            SyncLookupTableLength (length);

            int ln = Log2 (length);

            // reorder array
            ReorderArray (data);

            // successive doubling
            int n = 1;
            int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;

            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                double[] uRLookup = _uRLookup[level, signIndex];
                double[] uILookup = _uILookup[level, signIndex];

                for (int j = 0; j < m; j ++)
                {
                    double uR = uRLookup[j];
                    double uI = uILookup[j];

                    for (int even = j; even < length; even += n)
                    {
                        int odd = even + m;

                        double r = data[odd].Re;
                        double i = data[odd].Im;

                        double odduR = r * uR - i * uI;
                        double odduI = r * uI + i * uR;

                        r = data[even].Re;
                        i = data[even].Im;

                        data[even].Re = r + odduR;
                        data[even].Im = i + odduI;

                        data[odd].Re = r - odduR;
                        data[odd].Im = i - odduI;
                    }
                }
            }
        }

        /// <summary>
        /// Compute a 1D fast Fourier transform of a dataset of complex numbers.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void FftQuick (Complex[] data, int length, FourierDirection direction)
        {
            /*if( data == null ) {
				throw new ArgumentNullException( "data" );
			}
			if( data.Length < length ) {
				throw new ArgumentOutOfRangeException( "length", length, "must be at least as large as 'data.Length' parameter" );
			}
			if( Fourier.IsPowerOf2( length ) == false ) {
				throw new ArgumentOutOfRangeException( "length", length, "must be a power of 2" );
			}

			Fourier.SyncLookupTableLength( length );   */

            int ln = Log2 (length);

            // reorder array
            ReorderArray (data);

            // successive doubling
            int n = 1;
            int signIndex = (direction == FourierDirection.Forward) ? 0 : 1;

            for (int level = 1; level <= ln; level ++)
            {
                int m = n;
                n <<= 1;

                double[] uRLookup = _uRLookup[level, signIndex];
                double[] uILookup = _uILookup[level, signIndex];

                for (int j = 0; j < m; j ++)
                {
                    double uR = uRLookup[j];
                    double uI = uILookup[j];

                    for (int even = j; even < length; even += n)
                    {
                        int odd = even + m;

                        double r = data[odd].Re;
                        double i = data[odd].Im;

                        double odduR = r * uR - i * uI;
                        double odduI = r * uI + i * uR;

                        r = data[even].Re;
                        i = data[even].Im;

                        data[even].Re = r + odduR;
                        data[even].Im = i + odduI;

                        data[odd].Re = r - odduR;
                        data[odd].Im = i - odduI;
                    }
                }
            }
        }

        /// <summary>
        /// Compute a 1D real-symmetric fast fourier transform.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="direction"></param>
        public static void Rfft (float[] data, FourierDirection direction)
        {
            if (data == null)
            {
                throw new ArgumentNullException ("data");
            }
            Rfft (data, data.Length, direction);
        }

        /// <summary>
        /// Compute a 1D real-symmetric fast fourier transform.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="direction"></param>
        public static void Rfft (float[] data, int length, FourierDirection direction)
        {
            if (data == null)
            {
                throw new ArgumentNullException ("data");
            }
            if (data.Length < length)
            {
                throw new ArgumentOutOfRangeException ("length", length, "must be at least as large as 'data.Length' parameter");
            }
            if (IsPowerOf2 (length) == false)
            {
                throw new ArgumentOutOfRangeException ("length", length, "must be a power of 2");
            }

            const float c1 = 0.5f;
            float c2;
            float theta = (float) (Math.PI / ((double) length / 2));

            if (direction == FourierDirection.Forward)
            {
                c2 = -0.5f;
                Fft (data, length / 2, direction);
            }
            else
            {
                c2 = 0.5f;
                theta = - theta;
            }

            float wtemp = (float) Math.Sin (0.5 * theta);
            float wpr = -2 * wtemp * wtemp;
            float wpi = (float) Math.Sin (theta);
            float wr = 1 + wpr;
            float wi = wpi;

            // do / undo packing
            for (int i = 1; i < length / 4; i ++)
            {
                int a = 2 * i;
                int b = length - 2 * i;
                float h1R = c1 * (data[a] + data[b]);
                float h1I = c1 * (data[a + 1] - data[b + 1]);
                float h2R = -c2 * (data[a + 1] + data[b + 1]);
                float h2I = c2 * (data[a] - data[b]);
                data[a] = h1R + wr * h2R - wi * h2I;
                data[a + 1] = h1I + wr * h2I + wi * h2R;
                data[b] = h1R - wr * h2R + wi * h2I;
                data[b + 1] = -h1I + wr * h2I + wi * h2R;
                wr = (wtemp = wr) * wpr - wi * wpi + wr;
                wi = wi * wpr + wtemp * wpi + wi;
            }

            if (direction == FourierDirection.Forward)
            {
                float hir = data[0];
                data[0] = hir + data[1];
                data[1] = hir - data[1];
            }
            else
            {
                float hir = data[0];
                data[0] = c1 * (hir + data[1]);
                data[1] = c1 * (hir - data[1]);
                Fft (data, length / 2, direction);
            }
        }
    }
}